#include "push_rod.h"
#include "bsp_adc.h"
#include "delay.h"




static PushRod_t push_rod_one = {.TIMx = TIM2,.speed = 0,.TIM_SetCompare_One = TIM_SetCompare3,.TIM_SetCompare_Two = TIM_SetCompare4};
static PushRod_t push_rod_two = {.TIMx = TIM3,.speed = 0,.TIM_SetCompare_One = TIM_SetCompare1,.TIM_SetCompare_Two = TIM_SetCompare2};



static uint8_t push_rod_finishflag = 0x00;
static uint8_t push_rod_targerflag = 0x00;




void SD_GPIO_Config(void)
{
	GPIO_InitTypeDef  GPIO_InitStructure;
	RCC_AHB1PeriphClockCmd(PUSH1ROD_SD1_CLK|PUSH1ROD_SD2_CLK|PUSH2ROD_SD1_CLK|PUSH2ROD_SD2_CLK,ENABLE);

	/*********************************SD*************************************/
	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
	
	GPIO_InitStructure.GPIO_Pin   = PUSH1ROD_SD1_PIN;
	GPIO_Init(PUSH1ROD_SD1_PORT, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin   = PUSH1ROD_SD2_PIN;
	GPIO_Init(PUSH1ROD_SD2_PORT, &GPIO_InitStructure);
	
	GPIO_InitStructure.GPIO_Pin   = PUSH2ROD_SD1_PIN;
	GPIO_Init(PUSH2ROD_SD1_PORT, &GPIO_InitStructure);
	
	GPIO_InitStructure.GPIO_Pin   = PUSH2ROD_SD2_PIN;
	GPIO_Init(PUSH2ROD_SD2_PORT, &GPIO_InitStructure);
	
	
	/*********************TIM2-CH3-CH4*************************/
	push_rod_one.SD1.GPIOx        = PUSH1ROD_SD1_PORT;     
	push_rod_one.SD1.GPIO_Pin     = PUSH1ROD_SD1_PIN;
	push_rod_one.SD1.Polarity     = POS;
	
	push_rod_one.SD2.GPIOx        = PUSH1ROD_SD2_PORT;
	push_rod_one.SD2.GPIO_Pin     = PUSH1ROD_SD2_PIN;
	push_rod_one.SD2.Polarity     = POS;
	
	/*********************TIM3-CH1-CH2*************************/
	push_rod_two.SD1.GPIOx        = PUSH2ROD_SD1_PORT;
	push_rod_two.SD1.GPIO_Pin     = PUSH2ROD_SD1_PIN;
	push_rod_two.SD1.Polarity     = POS;
	
	push_rod_two.SD2.GPIOx        = PUSH2ROD_SD2_PORT;
	push_rod_two.SD2.GPIO_Pin     = PUSH2ROD_SD2_PIN;
	push_rod_two.SD2.Polarity     = POS;
}





void SetPushRodSpeed(uint8_t ID,int16_t speed)
{  
	if(ID == PUSH_ROD_ONE) push_rod_one.speed = speed;
	else if(ID == PUSH_ROD_TWO) push_rod_two.speed = speed;
}


static void push_rod_ctrl(PushRod_t *Push)
{	
	if(Push->speed >= MAX_SPEED)        Push->speed = MAX_SPEED;
	else if(Push->speed < (-MAX_SPEED)) Push->speed = -MAX_SPEED;
	
	
	if(Push->speed > 0)
	{
		GPIO_Out(&Push->SD1,OPEN);
		GPIO_Out(&Push->SD2,OPEN);
		delay_us(1);
		Push->TIM_SetCompare_One(Push->TIMx,Push->speed);
		Push->TIM_SetCompare_Two(Push->TIMx,0);
	}
	else if(Push->speed < 0)
	{
		GPIO_Out(&Push->SD1,OPEN);
		GPIO_Out(&Push->SD2,OPEN);
		delay_us(1);
		Push->TIM_SetCompare_One(Push->TIMx,0);
		Push->TIM_SetCompare_Two(Push->TIMx,(-Push->speed));
	}
	else if(Push->speed == 0)
	{
		GPIO_Out(&Push->SD1,CLOSE);
		GPIO_Out(&Push->SD2,CLOSE);
		delay_us(1);
		Push->TIM_SetCompare_One(Push->TIMx,0);
		Push->TIM_SetCompare_Two(Push->TIMx,0);
	}
	
}


float push_gan1_cur = 0,push_gan2_cur = 0;

void PUSH_ROD_CTRL(void)
{
	push_rod_ctrl(&push_rod_one);
	push_rod_ctrl(&push_rod_two);
    push_gan1_cur = PUSH1_CURRENT;
    push_gan2_cur = PUSH2_CURRENT;
}

void PUSH_ROD_SETFLAG(uint8_t ID)
{
	push_rod_targerflag = push_rod_targerflag | ID;
}

uint8_t PUSH_ROD_FINISHFLAG(void)
{
	if(push_gan1_cur < 0.01f)
		push_rod_finishflag = (push_rod_finishflag | PUSH_ROD_ONE)&0xFF; 
	if(push_gan2_cur < 0.01f)
		push_rod_finishflag = (push_rod_finishflag | PUSH_ROD_TWO)&0xFF;
	if(push_rod_targerflag == push_rod_finishflag){
        push_rod_finishflag = 0;
        return 1;
    }
    else{
        return 0;
    }
}



